#!/usr/bin/env python3
"""
Vision Interaction System Launch File - 优化版（集成网络自动发现）

完整的多模态交互系统：语音控制 + 视觉识别
自动启用视觉集成模式，支持智能命令解析和结构化输出
自动发现网络设备IP地址，无需手动配置

Features:
- 🔍 自动网络设备发现（EchoKit服务器、机器人、摄像头）
- ✅ 自动启用视觉集成模式（enable_vision_integration=true）
- 📹 ESP32摄像头支持（CompressedImage格式）
- 🤖 Coze AI视觉识别
- 🧠 智能命令解析和结构化输出
- 🎯 语音-视觉多模态协同
- 🔄 本地视觉模型备份（可选）
- 🛡️ 后备机制：发现失败时使用默认IP

Architecture:
  语音控制: EchoKit LLM → WebSocket → llm_bridge → /cmd_vel → 机器人
  视觉识别: ESP32-CAM → /camera/image_raw/compressed → vision_coze_node → Coze API → /vision_result
  多模态: 语音命令 → voice_vision_integration → 触发视觉查询 → 语音反馈
"""

import os
import sys
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

# 导入网络自动发现工具
try:
    from echokitbot_microros_voice.network_discovery import NetworkDiscovery
    NETWORK_DISCOVERY_AVAILABLE = True
except ImportError:
    print("⚠️  警告: 网络自动发现工具不可用，将使用默认IP地址")
    NETWORK_DISCOVERY_AVAILABLE = False


def generate_launch_description():
    # ========== 网络设备自动发现 ==========
    print("\n" + "="*70)
    print("🔍 网络设备自动发现")
    print("="*70)
    
    # 默认IP地址（后备方案，已替换为占位符）
    default_echokit_ip = "<ECHO_SERVER>"
    default_robot_ip = "<ECHOKIT_IP>"
    default_camera_ip = "<CAMERA_IP>"
    
    # 尝试自动发现设备IP
    if NETWORK_DISCOVERY_AVAILABLE:
        try:
            discovery = NetworkDiscovery(timeout=0.5, verbose=True)
            
            # 发现EchoKit服务器
            print("\n🔍 正在发现EchoKit服务器...")
            echokit_ip = discovery.discover_echokit_server(
                default_ip=default_echokit_ip,
                ports=[8060]
            )
            
            # 发现机器人
            print("\n🔍 正在发现机器人...")
            robot_ip = discovery.discover_robot(
                default_ip=default_robot_ip
            )
            
            # 发现摄像头
            print("\n🔍 正在发现摄像头...")
            camera_ip = discovery.discover_camera(
                default_ip=default_camera_ip,
                ports=[8554, 81]
            )
            
            print("\n✅ 设备发现完成:")
            print(f"  📡 EchoKit服务器: {echokit_ip}")
            print(f"  🤖 机器人: {robot_ip}")
            print(f"  📹 摄像头: {camera_ip}")
            
        except Exception as e:
            print(f"\n⚠️  自动发现失败: {e}")
            print("使用默认IP地址...")
            echokit_ip = default_echokit_ip
            robot_ip = default_robot_ip
            camera_ip = default_camera_ip
    else:
        print("\n⚠️  网络自动发现不可用，使用默认IP地址")
        echokit_ip = default_echokit_ip
        robot_ip = default_robot_ip
        camera_ip = default_camera_ip
    
    print("="*70 + "\n")
    
    # 支持环境变量覆盖
    echokit_ip = os.environ.get('ECHOKIT_SERVER_IP', echokit_ip)
    robot_ip = os.environ.get('ROBOT_IP', robot_ip)
    camera_ip = os.environ.get('CAMERA_IP', camera_ip)
    
    # 构建完整URL
    server_url_default = f"ws://{echokit_ip}:8060/ws/robot"
    
    # Declare launch arguments
    config_file_arg = DeclareLaunchArgument(
        'config_file',
        default_value=PathJoinSubstitution([
            FindPackageShare('echokitbot_microros_voice'),
            'config',
            'vision_system_config.yaml'
        ]),
        description='Path to the vision system configuration file'
    )
    
    enable_local_model_arg = DeclareLaunchArgument(
        'enable_local_model',
        default_value='false',
        description='Enable local vision model as fallback'
    )
    
    server_url_arg = DeclareLaunchArgument(
        'server_url',
        default_value=server_url_default,
        description='EchoKit server WebSocket URL (auto-discovered or default)'
    )
    
    robot_ip_arg = DeclareLaunchArgument(
        'robot_ip',
        default_value=robot_ip,
        description='Robot IP address (auto-discovered or default)'
    )
    
    camera_ip_arg = DeclareLaunchArgument(
        'camera_ip',
        default_value=camera_ip,
        description='Camera IP address (auto-discovered or default)'
    )
    
    log_level_arg = DeclareLaunchArgument(
        'log_level',
        default_value='info',
        description='Log level (debug, info, warn, error)'
    )
    
    coze_api_key_arg = DeclareLaunchArgument(
        'coze_api_key',
        default_value=os.environ.get('COZE_API_KEY', ''),
        description='Coze API key for vision analysis'
    )
    
    coze_bot_id_arg = DeclareLaunchArgument(
        'coze_bot_id',
        default_value=os.environ.get('COZE_BOT_ID', ''),
        description='Coze bot ID for vision analysis'
    )

    # ========== 摄像头节点 ==========
    # ESP32摄像头节点 - 发布压缩图像到 /camera/image_raw/compressed
    esp32_camera_node = Node(
        package='echokitbot_microros_voice',
        executable='esp32_camera_node',
        name='esp32_camera_node',
        parameters=[{
            'quality': 18,
            'framesize': 5,  # 320x240
            'udp_port': 8887,
            'stream_port': 81,
        }],
        output='screen',
        emulate_tty=True,
    )

    # ========== 视觉识别节点 ==========
    # Vision Coze节点 - 订阅摄像头图像，调用Coze API进行识别
    vision_coze_node = Node(
        package='echokitbot_microros_voice',
        executable='vision_coze_node',
        name='vision_coze_node',
        parameters=[{
            'coze_api_key': LaunchConfiguration('coze_api_key'),
            'coze_bot_id': LaunchConfiguration('coze_bot_id'),
            'enable_auto_save': False,
        }],
        output='screen',
        emulate_tty=True,
        arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')]
    )

    # ========== 语音控制系统（自动启用视觉集成）==========
    # LLM Bridge Voice Control Node - 连接EchoKit Server，自动启用视觉集成
    llm_bridge_node = Node(
        package='echokitbot_microros_voice',
        executable='llm_bridge_voice_control_node',
        name='llm_bridge_voice_control_node',
        parameters=[{
            'server_url': LaunchConfiguration('server_url'),
            'reconnect_interval': 10.0,
            'connection_timeout': 30.0,
            'enable_vision_integration': True,  # 自动启用视觉集成
            'movement.max_linear_velocity': 0.5,
            'movement.max_angular_velocity': 1.0,
            'safety.command_timeout': 5.0,
        }],
        output='screen',
        emulate_tty=True,
        arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')]
    )

    # LLM Enhanced Voice Processor - 智能命令解析和结构化输出
    llm_enhanced_processor = Node(
        package='echokitbot_microros_voice',
        executable='llm_enhanced_voice_processor',
        name='llm_enhanced_voice_processor',
        parameters=[{
            'motion_config.linear_speed': 0.3,
            'motion_config.angular_speed': 1.0,
            'motion_config.angle_tolerance': 5.0,
            'motion_config.distance_tolerance': 0.05,
            'enable_structured_output': True,  # 启用结构化输出
            'enable_smart_parsing': True,      # 启用智能解析
        }],
        output='screen',
        emulate_tty=True,
        arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')]
    )

    # ========== 多模态协同节点 ==========
    # Voice-Vision Integration Bridge - 语音触发视觉查询
    voice_vision_bridge = Node(
        package='echokitbot_microros_voice',
        executable='voice_vision_integration',
        name='voice_vision_integration',
        parameters=[{
            'voice_trigger_keywords': ['你看到了什么', '看到什么', '描述一下', '前面有什么', '这是什么', '识别', '看看'],
            'vision_response_timeout': 10.0,
            'enable_auto_feedback': True,  # 自动语音反馈
        }],
        output='screen',
        emulate_tty=True,
        arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')]
    )

    # System Monitor - 系统健康监控（包含视觉系统）
    system_monitor = Node(
        package='echokitbot_microros_voice',
        executable='system_monitor',
        name='system_monitor',
        parameters=[{
            'echokitbot_ip': LaunchConfiguration('robot_ip'),
            'echokit_server': f'{echokit_ip}:8060',
            'monitor_vision_system': True,  # 监控视觉系统
            'monitor_camera': True,         # 监控摄像头
        }],
        output='screen',
        emulate_tty=True,
        arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')]
    )

    # ========== 本地视觉模型（可选备份）==========
    local_vision_model_node = Node(
        package='echokitbot_microros_voice',
        executable='local_vision_model',
        name='local_vision_model',
        parameters=[{
            'model_type': 'clip',
            'enable_fallback': True,
        }],
        output='screen',
        emulate_tty=True,
        arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')],
        condition=IfCondition(LaunchConfiguration('enable_local_model'))
    )

    return LaunchDescription([
        # Launch arguments
        config_file_arg,
        enable_local_model_arg,
        server_url_arg,
        robot_ip_arg,
        camera_ip_arg,
        log_level_arg,
        coze_api_key_arg,
        coze_bot_id_arg,
        
        # Startup messages
        LogInfo(msg=''),
        LogInfo(msg='╔════════════════════════════════════════════════════════════════╗'),
        LogInfo(msg='║   Vision Interaction System - 多模态交互系统（优化版）        ║'),
        LogInfo(msg='║   🔍 集成网络自动发现功能                                     ║'),
        LogInfo(msg='╚════════════════════════════════════════════════════════════════╝'),
        LogInfo(msg=''),
        LogInfo(msg='🎯 系统特性:'),
        LogInfo(msg='  ✅ 自动网络设备发现（无需手动配置IP）'),
        LogInfo(msg='  ✅ 自动启用视觉集成模式'),
        LogInfo(msg='  ✅ 智能命令解析和结构化输出'),
        LogInfo(msg='  ✅ ESP32摄像头支持（CompressedImage）'),
        LogInfo(msg='  ✅ Coze AI视觉识别'),
        LogInfo(msg='  ✅ 语音-视觉多模态协同'),
        LogInfo(msg=''),
        LogInfo(msg='📡 已发现的设备:'),
        LogInfo(msg=['  EchoKit服务器: ', server_url_default]),
        LogInfo(msg=['  机器人: ', robot_ip]),
        LogInfo(msg=['  摄像头: ', camera_ip]),
        LogInfo(msg=''),
        LogInfo(msg='💡 提示: 可通过环境变量覆盖:'),
        LogInfo(msg='  export ECHOKIT_SERVER_IP=<ip>'),
        LogInfo(msg='  export ROBOT_IP=<ip>'),
        LogInfo(msg='  export CAMERA_IP=<ip>'),
        LogInfo(msg=''),
        LogInfo(msg='📡 系统架构:'),
        LogInfo(msg=['  语音: EchoKit Server (', LaunchConfiguration('server_url'), ')']),
        LogInfo(msg='  摄像头: ESP32-CAM → /camera/image_raw/compressed'),
        LogInfo(msg='  视觉AI: vision_coze_node → Coze API → /vision_result'),
        LogInfo(msg='  协同: voice_vision_integration (自动桥接)'),
        LogInfo(msg=''),
        LogInfo(msg='🎤 支持的语音命令:'),
        LogInfo(msg='  运动控制: "前进2米", "左转90度", "后退1米"'),
        LogInfo(msg='  视觉查询: "你看到了什么", "前面有什么", "这是什么"'),
        LogInfo(msg='  多模态: "向左边的红色物体移动"'),
        LogInfo(msg=''),
        LogInfo(msg='⚙️  配置检查:'),
        LogInfo(msg=['  COZE_API_KEY: ', 'configured' if os.environ.get('COZE_API_KEY') else '❌ NOT SET']),
        LogInfo(msg=['  COZE_BOT_ID: ', 'configured' if os.environ.get('COZE_BOT_ID') else '❌ NOT SET']),
        LogInfo(msg=''),
        
        # Launch nodes
        esp32_camera_node,
        vision_coze_node,
        llm_bridge_node,
        llm_enhanced_processor,
        voice_vision_bridge,
        system_monitor,
        local_vision_model_node,
    ])